/*
 *************************************************************************************************************
 *                                                uC/OS-II
 *                                          The Real-Time Kernel
 *
 *                                         ATmega2560  Sample code
 *
 * File : APP.C
 * By   : Fabiano Kovalski - portet to ATMEGA2560 by Ib Havn, 2010
 *************************************************************************************************************
 */

#include  <includes.h>
#include <../lego_interface/lego_interface.h>
#include <toolbox/toolbox.h>
#include "legotasks.h"


/*
 **************************************************************************************************************
 *                                               VARIABLES
 **************************************************************************************************************
 */

OS_STK AppTaskStartStk[OS_TASK_START_STK_SIZE];
OS_STK first_belt_runStk[OS_UNIVERSAL_STK_SIZE];
OS_STK sim_brickStk[OS_UNIVERSAL_STK_SIZE];
OS_STK rotate_unto_nextStk[OS_UNIVERSAL_STK_SIZE];
OS_STK second_belt_runStk[OS_UNIVERSAL_STK_SIZE];
OS_STK touch_sensor_readerStk[OS_UNIVERSAL_STK_SIZE];
OS_STK run_sorterStk[OS_UNIVERSAL_STK_SIZE];

OS_EVENT *a_sem; // Pointer to a semaphore


OS_EVENT *first_belt_sem; //semaphore to first belt(buffer)
OS_EVENT *sort_sem;

INT8U On_flag_first_belt;
INT8U On_second_first_belt;


//pointer variables to tasks
void (*pt_sim_brick)(void *p_arg);
void (*pt_first_belt_run)(void *p_arg);
void (*pt_rotate_unto_next)(void *p_arg);
void (*pt_second_belt_run) (void *p_arg);
void (*pt_touch_sensor_reader) (void *p_arg);
void (*pt_run_sorter) (void *p_arg);

//our semaphore/buffer value
static INT8U buffer_value;
/*
 **************************************************************************************************************
 *                                           FUNCTION PROTOTYPES
 **************************************************************************************************************
 */

static void AppTaskStart(void *p_arg);
static void AppTaskCreate(void);




/*
 **************************************************************************************************************
 *                                                MAIN
 *
 * Note(s): 1) You SHOULD use OS_TASK_STK_SIZE (see OS_CFG.H) when setting OSTaskStkSize prior to calling
 *             OSInit() because OS_TASK_IDLE_STK_SIZE and OS_TASK_STAT_STK_SIZE are set to this value in
 *             OS_CFG.H.
 **************************************************************************************************************
 */

int main(void) {
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	INT8U err;
#endif

	//DDRB = 0x0F; // TODO IHA Remove after test

	/*---- Any initialization code prior to calling OSInit() goes HERE -------------------------------------*/

	/* IMPORTANT: MUST be setup before calling 'OSInit()'  */
	OSTaskStkSize = OS_TASK_IDLE_STK_SIZE; /* Setup the default stack size                        */
	//    OSTaskStkSizeHard = OS_TASK_STK_SIZE_HARD;       /* Setup the default hardware stack size               */

	OSInit(); /* Initialize "uC/OS-II, The Real-Time Kernel"         */

	/*---- Any initialization code before starting multitasking --------------------------------------------*/
	OSTaskStkSize = OS_TASK_START_STK_SIZE;

	OSTaskCreateExt(AppTaskStart, (void *) 0,
			(OS_STK *) &AppTaskStartStk[OSTaskStkSize - 1], OS_TASK_START_PRIO,
			OS_TASK_START_PRIO, (OS_STK *) &AppTaskStartStk[0], OSTaskStkSize,
			(void *) 0, OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);

#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	OSTaskNameSet(OS_TASK_START_PRIO, "Start Task", &err);
#endif

	a_sem = OSSemCreate(0); // create a semaphore


	//initializing the lego board and belt
	init_lego_interface();

	//initializing the buffer value
	buffer_value = 2;

	//semaphore for first belt
	first_belt_sem = OSSemCreate(buffer_value);
	sort_sem = OSSemCreate(1);

	//set first belt flag so its runs from the beginning
	On_flag_first_belt = 1;
	On_second_first_belt = 1;


	//the following function inits the variables in legotasks.c
	init_tasks(first_belt_sem,sort_sem, On_flag_first_belt, On_second_first_belt, buffer_value);


	//initialization of pointers to task
	pt_sim_brick = &sim_brick;
	pt_first_belt_run = &first_belt_run;
	pt_rotate_unto_next = &rotate_unto_next;
	pt_second_belt_run = &second_belt;
	pt_touch_sensor_reader = &touch_sensor_reader;
	pt_run_sorter = &run_sorter;



	/*---- Create any other task you want before we start multitasking -------------------------------------*/

	OSStart(); /* Start multitasking (i.e. give control to uC/OS-II)  */
	return 0;
}

/*
 **************************************************************************************************************
 *                                              STARTUP TASK
 *
 * Description : This is an example of a startup task.  As mentioned in the book's text, you MUST
 *               initialize the ticker only once multitasking has started.
 *
 * Arguments   : p_arg   is the argument passed to 'AppStartTask()' by 'OSTaskCreate()'.
 *
 * Notes       : 1) The first line of code is used to prevent a compiler warning because 'p_arg' is not
 *                  used.  The compiler should not generate any code for this statement.
 **************************************************************************************************************
 */

static void AppTaskStart(void *p_arg) {
	INT8U i;
	INT8U err;

	(void) p_arg; /* Prevent compiler warnings                          */

	BSP_Init(); /* Initialize the BSP                                 */
	AppTaskCreate();

	while (1) { /* Task body, always written as an infinite loop.     */
		for (i = 1; i <= 6; i++) {
			LED_On(i);
			OSTimeDly(OS_TICKS_PER_SEC / 10);
			LED_Off(i);
		}
		for (i = 5; i > 1; i--) {
			LED_On(i);
			OSTimeDly(OS_TICKS_PER_SEC / 10);
			LED_Off(i);
		}
		OSSemPend(a_sem, 0, &err); // Wait for the semaphore to be signaled
	}
}

/*
 **************************************************************************************************************
 *                                        CREATE APPLICATION TASKS
 *
 * Description : This function creates the application tasks.
 *
 * Arguments   : p_arg   is the argument passed to 'AppStartTask()' by 'OSTaskCreate()'.
 *
 * Notes       : 1) The first line of code is used to prevent a compiler warning because 'p_arg' is not
 *                  used.  The compiler should not generate any code for this statement.
 **************************************************************************************************************
 */



static void AppTaskCreate(void) {
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	INT8U err;
#endif

	/*---- Task initialization code goes HERE! --------------------------------------------------------*/
	OSTaskStkSize = OS_UNIVERSAL_STK_SIZE; /* Setup the default stack size                     */
	//    OSTaskStkSizeHard = OS_TASK_STK_SIZE_HARD;     /* Setup the default hardware stack size            */
	OSTaskCreateExt(pt_first_belt_run, (void *) 0,
			(OS_STK *) &first_belt_runStk[OSTaskStkSize - 1],
			first_belt_run_prio, first_belt_run_prio,
			(OS_STK *) &first_belt_runStk[0], OSTaskStkSize, (void *) 0,
			OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	OSTaskNameSet(OS_TASK_1_PRIO, "Task 1", &err);
#endif


	/*---- Task initialization code goes HERE! --------------------------------------------------------*/
	OSTaskStkSize = OS_UNIVERSAL_STK_SIZE; /* Setup the default stack size                     */
	//    OSTaskStkSizeHard = OS_TASK_STK_SIZE_HARD;     /* Setup the default hardware stack size            */
	OSTaskCreateExt(pt_sim_brick, (void *) 0,
			(OS_STK *) &sim_brickStk[OSTaskStkSize - 1], sim_brick_prio,
			sim_brick_prio, (OS_STK *) &sim_brickStk[0], OSTaskStkSize,
			(void *) 0, OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	OSTaskNameSet(OS_TASK_1_PRIO, "Task 1", &err);
#endif

	/*---- Task initialization code goes HERE! --------------------------------------------------------*/
	OSTaskStkSize = OS_UNIVERSAL_STK_SIZE; /* Setup the default stack size                     */
	//    OSTaskStkSizeHard = OS_TASK_STK_SIZE_HARD;     /* Setup the default hardware stack size            */
	OSTaskCreateExt(pt_rotate_unto_next, (void *) 0,
			(OS_STK *) &rotate_unto_nextStk[OSTaskStkSize - 1], rotate_unto_next_prio,
			rotate_unto_next_prio, (OS_STK *) &rotate_unto_nextStk[0], OSTaskStkSize,
			(void *) 0, OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	OSTaskNameSet(OS_TASK_1_PRIO, "Task 1", &err);
#endif

	/*---- Task initialization code goes HERE! --------------------------------------------------------*/
	OSTaskStkSize = OS_UNIVERSAL_STK_SIZE; /* Setup the default stack size                     */
	//    OSTaskStkSizeHard = OS_TASK_STK_SIZE_HARD;     /* Setup the default hardware stack size            */
	OSTaskCreateExt(pt_second_belt_run, (void *) 0,
			(OS_STK *) &second_belt_runStk[OSTaskStkSize - 1], second_belt_run_prio,
			second_belt_run_prio, (OS_STK *) &second_belt_runStk[0], OSTaskStkSize,
			(void *) 0, OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	OSTaskNameSet(OS_TASK_1_PRIO, "Task 1", &err);
#endif

	/*---- Task initialization code goes HERE! --------------------------------------------------------*/
	OSTaskStkSize = OS_UNIVERSAL_STK_SIZE; /* Setup the default stack size                     */
	//    OSTaskStkSizeHard = OS_TASK_STK_SIZE_HARD;     /* Setup the default hardware stack size            */
	OSTaskCreateExt(pt_touch_sensor_reader, (void *) 0,
			(OS_STK *) &touch_sensor_readerStk[OSTaskStkSize - 1], touch_sensor_reader_prio,
			touch_sensor_reader_prio, (OS_STK *) &touch_sensor_readerStk[0], OSTaskStkSize,
			(void *) 0, OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	OSTaskNameSet(OS_TASK_1_PRIO, "Task 1", &err);
#endif


	/*---- Task initialization code goes HERE! --------------------------------------------------------*/
	OSTaskStkSize = OS_UNIVERSAL_STK_SIZE; /* Setup the default stack size                     */
	//    OSTaskStkSizeHard = OS_TASK_STK_SIZE_HARD;     /* Setup the default hardware stack size            */
	OSTaskCreateExt(pt_run_sorter, (void *) 0,
			(OS_STK *) &run_sorterStk[OSTaskStkSize - 1], run_sorter_prio,
			run_sorter_prio, (OS_STK *) &run_sorterStk[0], OSTaskStkSize,
			(void *) 0, OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);
#if (OS_TASK_NAME_SIZE > 14) && (OS_TASK_STAT_EN > 0)
	OSTaskNameSet(OS_TASK_1_PRIO, "Task 1", &err);
#endif

}
/*
 *********************************************************************************************************
 *                                           TASK SWITCH HOOK
 *
 * Description: This function is called when a task switch is performed.  This allows you to perform other
 *              operations during a context switch.
 *
 * Arguments  : none
 *
 * Note(s)    : 1) Interrupts are disabled during this call.
 *              2) It is assumed that the global pointer 'OSTCBHighRdy' points to the TCB of the task that
 *                 will be 'switched in' (i.e. the highest priority task) and, 'OSTCBCur' points to the
 *                 task being switched out (i.e. the preempted task).
 *********************************************************************************************************
 */
void App_TaskSwHook(void) // TODO IHA Remove after test
{
	//PORTB = ~(OSTCBHighRdy->OSTCBPrio);
}

void App_TaskCreateHook(OS_TCB *ptcb) {
}
void App_TaskDelHook(OS_TCB *ptcb) {
}
void App_TaskIdleHook(void) {
}
void App_TaskStatHook(void) {
}
void App_TCBInitHook(OS_TCB *ptcb) {
}
void App_TimeTickHook(void) {
}
